/**
 * @file drv_timer.c
 * @brief Code for drv_timer driver
 * @author He Shaohua (hsh15529870639@foxmail.com)
 * @date 2024-05-20
 * @version 1.0
 * @copyright Copyright (c) 2024, SNUT-Chaoyue Team
 * ****************************************************************************************************
 * @par Change Log:
 * <table>
 * <tr><th>Date         <th>Version       <th>Author      <th>Description
 * <tr><td>2024-05-20   <td> 1.0          <td>He Shaohua  <td>Creator
 *
 * </table>
 * ==================================----How to use this driver----=====================================
 * [..]
 * @note
 *   -#
 *
 * @warning
 *   -#
 *
 * *****************************************************************************************************
 * @attention
 *
 * This document is required and please understand any bugs that may occur in the document.
 * If you want to modify this file, after the code update is completed, update the version number in time,
 * indicate the update date, and fill in the changes
 * Make sure that the submitted code does not have many bugs.
 *
 * All rights reserved.
 * *****************************************************************************************************
 */

/* Includes ------------------------------------------------------------------*/
#include "alg_pid.h"
#include <stdlib.h>
/* Private typedef -----------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
#define abs(x) ((x > 0) ? x : -x)
/* Private variables ---------------------------------------------------------*/
static uint8_t idx = 0;
static PID_Increase_Instance *pid_instance[4] = {NULL};
/* Private function prototypes -----------------------------------------------*/
static void _pid_Constrain(float *x, float Min, float Max)
{
  if (*x < Min)
    *x = Min;
  if (*x > Max)
    *x = Max;
}
/* Private functions ---------------------------------------------------------*/

PID_Increase_Instance *
PIDInit(PID_Increase_Param_Config_Typedef *config, PID_Increase_Mode_Typedef mode)
{
  PID_Increase_Instance *instance = (PID_Increase_Instance *)malloc(sizeof(PID_Increase_Instance));
  memset(instance, 0, sizeof(PID_Increase_Instance));

  instance->K_p = config->K_p;
  instance->K_i = config->K_i;
  instance->K_d = config->K_d;
  instance->i_limit = config->i_limit;
  instance->out_limit_high = config->out_limit_high;
  instance->out_limit_low = config->out_limit_low;
  instance->dead_zone = config->dead_zone;
  instance->Derivative_LPF_RC = config->Derivative_LPF_RC;
  instance->mode = mode;

  TIMER_GetDeltaT(&instance->last_time);

  pid_instance[idx++] = instance;

  return instance;
}

//
float PIDCalculate(PID_Increase_Instance *pid, float measure, float Target)
{
  float p_out = 0.0f, i_out = 0.0f, d_out = 0.0f, pid_out = 0.0f;
  float Delta_Time = 0.0f;
  float error = 0.0f, abs_error = 0.0f;
  Delta_Time = TIMER_GetDeltaT(&(pid->last_time));

  error = Target - measure;

  if (Delta_Time < 0.001f || Delta_Time > 0.1f)
  {
    pid_out = pid->last_output;
  }
  else
  {
    // 判断死区
    if ((pid->mode & Dead_Zone) && (pid->dead_zone > abs(error)))
    {
      error = 0.0f;
    }

    // 计算P项
    p_out = pid->K_p * (error - pid->last_error);

    if (abs(pid->K_i) > 0)
    {
      // 计算I项
      i_out = error * Delta_Time * pid->K_i;
      if (pid->mode & Trapezoid_Intergral)
      {
        i_out = pid->K_i * Delta_Time * (error + pid->last_error) * 0.5f;
      }
      _pid_Constrain(&i_out, -(pid->i_limit), pid->i_limit);
    }

    if (abs(pid->K_d) > 0)
    { // 计算D项
      d_out = (error - 2 * pid->last_error + pid->pre_error) / Delta_Time;
      if (pid->mode & DerivationFilter)
      {
        d_out = Delta_Time / (Delta_Time + pid->Derivative_LPF_RC) * d_out +
                pid->d_out * pid->Derivative_LPF_RC /
                    (Delta_Time + pid->Derivative_LPF_RC);
      }
      pid->d_out = d_out;
    }

    // 错序更新
    pid->pre_error = pid->last_error;
    pid->last_error = error;

    pid_out = p_out + i_out + d_out;

    // 输出限幅
    _pid_Constrain(&pid_out, -(pid->out_limit_high), pid->out_limit_high);

    if (abs(pid_out) < pid->out_limit_low)
    {
      if (pid_out < 0.0f)
      {
        pid_out = -pid->out_limit_low;
      }
      else
      {
        pid_out = pid->out_limit_low;
      }
    }

    pid->last_output = pid_out;
  }
  return pid_out;
}

void PIDReset(PID_Increase_Instance *pid)
{
  pid->last_error = 0.0f;
  pid->pre_error = 0.0f;
  pid->last_output = 0.0f;
  pid->d_out = 0.0f;
}
/**
 * @}
 */

/**
 * @}
 */

/*********************************END OF FILE*********************************/
